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ABSTRACT 

This  report  presents  a  review  of  recent  non-linear  and  robust  filtering  results 
for  stochastic  systems.  We  focus  on  stability  and  robustness  issues  that  arise 
in  the  filtering  of  real  systems.  Issues  such  as  numeric  stability  and  the  effect 
of  non-linearity  are  also  considered. 

The  report  begins  by  introducing  the  famous  Kalman  filtering  problem 
before  proceeding  to  introduce  the  extended  Kalman  filter  and  related  stability 
results.  Robust  forms  of  the  Kalman  filter  and  extended  Kalman  filter  are  also 
considered  and  finally  a  particle  filtering  approach  is  presented. 

The  report  is  intended  to  lead  readers  with  a  familiarity  of  the  Kalman  fil¬ 
tering  problem  through  some  of  the  more  important  recent  (and  not  so  recent) 
results  on  stability  and  robust  filters  in  non-linear  filtering  problems. 
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Non-linear  and  Robust  Filtering:  From  the  Kalman  Filter 

to  the  Particle  Filter 

EXECUTIVE  SUMMARY 

In  the  context  of  signal  analysis,  filtering  is  the  process  of  separating  a  signal  of  interest 
from  other  signals,  termed  noise  signals.  All  filtering  of  signals  is  based  on  assumptions 
regarding  the  nature  of  the  interesting  signal  and  the  noise  signals.  Often  filters  are  used 
even  when  it  is  known  that  these  assumptions  do  not  hold.  In  this  report  we  present 
a  review  of  standard  filtering  techniques,  examine  techniques  for  improving  the  numeric 
stability  of  filters,  present  stochastic  stability  results  and  examine  filters  that  mitigate  for 
the  possibility  of  modelling  errors. 

Modern  guided  weapons  are  becoming  required  to  operate  in  highly  non-linear  and  time- 
varying  situations.  In  these  types  of  engagements,  assumptions  of  linearity  no  longer 
hold  and  model  uncertainties  in  the  form  of  unmeasured  aerodynamic  coefficients  and 
complex  non-linear  aerodynamics  are  common.  Filtering  is  an  important  sub-system  of 
any  guidance  loop  that  is  needed  to  estimate  required  engagement  information.  For  these 
reasons,  a  full  understanding  of  the  effects  of  non-linearity  and  model  uncertainty  on 
filtering  solutions  is  required. 

Tins  report  provides  a  review  of  existing  stochastic  filtering  results  from  the  well  known 
Kalman  filter  and  non-linear  and  robust  generalisation  of  the  Kalman  filter  through  to  new 
particle  filter  approaches  for  non-linear  state  estimation  problems.  Several  of  these  filters 
are  tested  in  simulations  of  a  typical  interceptor-target  engagement  to  examine  them  in  a 
guided  weapon  context. 

An  improved  understanding  of  filtering  techniques  is  required  to  aid  support  of  present 
upgrade  programs  involving  the  guidance  loops  of  new  standoff  missile  systems.  This 
understanding  is  necessary  for  the  support  of  future  weapon  procurement  and  upgrade 


programs. 
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1  Introduction 

The  purpose  of  filtering  is  to  separate  one  thing  from  another.  In  the  context  of  signal 
analysis,  filtering  is  the  process  of  separating  a  signal  of  interest  from  other  signals  termed, 
noise  signals.  All  filtering  of  signals  is  based  on  assumptions  regarding  the  nature  of  the 
interesting  signal  and  the  noise  signals.  A  system  model  is  a  collection  of  assumptions  that 
describe  the  relationship  between  the  signal  and  noise.  Ideally  the  assumed  system  model 
should  perfectly  describe  the  true  system  but  this  is  not  often  the  case.  Often  filters  are 
used  even  when  it  is  known  that  these  assumptions  do  not  hold.  In  this  report  we  examine 
how  the  performance  of  a  filtering  solution  is  effected  by  mismatch  between  the  assumed 
model  and  the  real  system. 

The  most  famous  and  commonly  used  assumptions  are  that  the  system  is  linear  Gauss- 
Markov  and  that  the  noises  are  Gaussian.  It  has  been  shown  that  under  these  assumptions 
the  Kalman  filter  is  the  optimal  filter  for  separating  the  interesting  signals  generated  by  a 
linear  Gauss-Markov  model  observed  in  Gaussian  noise  [1].  Because  the  Kalman  filter  is 
an  optimal  filter  (in  a  minimum  mean  squares  sense)  and  is  a  finite  dimensional  solution 
(can  be  implemented  using  a  finite  number  of  statistics  which  can  be  calculated  using  a 
finite  number  of  recursions)  it  has  been  applied  to  a  large  variety  of  filtering  problems. 
The  continuing  success  of  the  Kalman  filter  in  many  applications  has  encouraged  its  use 
even  in  situations  where  it  is  clear  that  the  system  is  non-linear. 

An  important  implementation  consideration  is  the  numeric  stability  of  any  filtering  algo¬ 
rithm.  Although  the  Kalman  filter  is  optimal  in  a  minimum  mean  least  squares  sense,  it 
has  been  shown  that  it  can  be  numerically  unstable  when  implemented  on  a  finite  word 
length  processor  [2].  Such  numeric  issues  are  probably  not  as  significant  today  as  they 
where  in  the  early  1960s  because  of  the  advances  on  computer  technology;  however,  in  some 
situations  the  numeric  problems  of  the  standard  form  of  the  Kalman  filter  may  still  be  an 
issue.  For  example,  in  very  low  noise  situations  the  numeric  errors  due  to  finite-precision 
arithmetic  can  result  in  the  standard  form  of  the  Kalman  filter  becoming  unstable  [2], 
This  report  gives  details  of  how  to  avoid  some  of  the  numeric  problems  inherent  in  the 
standard  form  of  the  Kalman  filter  by  implementing  a  numerically  stable  form  such  as  the 
U-D  covariance  form  of  the  Kalman  filter. 
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Apart  from  the  Kalman  filter,  there  are  very  few  finite-dimensional  optimal  filters  for 
stochastic  filtering  problems.  For  general  non-linear  problems,  when  a  finite- dimensional 
optimal  filter  is  not  possible,  sub-optimal  numeric  or  approximate  approaches  must  be 
used.  The  simplest  approach  is  to  linearise  the  non-linear  model  about  various  operating 
points  and  to  use  an  extension  of  the  Kalman  filter,  known  as  the  extended  Kalman  filter 
(EKF).  Details  of  the  extended  Kalman  filter  are  given  in  this  report  as  well  as  some  recent 
stability  results  that  establish  under  what  conditions  the  extended  Kalman  filter  will  give 
reasonable  filtering  solutions. 

Both  the  Kalman  filter  and  the  extended  Kalman  filter  assume  that  true  system  model  is 
known  with  certainty  for  design  purposes.  This  is  unlikely  in  most  practical  situations.  It 
is  more  likely  that  the  system  model  will  not  be  known  with  complete  certainty.  Kalman 
filtering  theory  does  not  establish  how  the  Kalman  filter  performs  if  system  assumptions 
are  incorrect.  In  fact,  even  a  very  slight  error  in  the  system  model  can  result  in  poor  filter 
performance.  The  question  of  how  to  design  filters  when  there  is  some  uncertainty  in  the 
system  model  is  addressed  by  robust  filtering  theory. 

In  this  report  we  present  some  recent  results  describing  the  robust  linear  Kalman  filter. 
The  robust  Kalman  filter  (or  set-valued  state  estimator)  allows  state  estimation  for  any 
member  of  a  structured  set  of  model  (with  bounded  rather  than  Gaussian  noise  terms). 
The  filter  is  termed  a  set- valued  state  estimator  because  it  determines  the  set  of  possible 
values  that  can  taken  on  by  the  state  process  given  the  true  system  is  one  from  a  structured 
set  of  models.  That  is,  it  determines  every  possible  state  sequence  that  can  match  the 
observations.  Under  the  model  uncertainty,  any  of  these  sequences  are  possible  and  the 
set  of  estimated  state  sequence  is  usually  represented  by  a  sequence  in  the  middle  of  the 
set. 

Although  not  considered  in  this  report,  there  are  several  other  approaches  that  allow  for 
some  model  uncertainty.  Risk-sensitive  filtering  is  an  optimal  filtering  approach  in  which 
the  filtering  criteria  is  modified  in  a  particular  way  to  mitigate  the  possibility  of  modelling 
error.  Risk-sensitive  filters  tend  to  have  better  conditional  mean  error  performance  than 
H°°  filtering  (but  less  than  optimal  conditional  mean  filters)  and  have  some  capability 
to  handle  modelling  errors  [11,  12,  13].  An  alternative  approach  is  mixed  criteria  state 
estimation  which  is  examined  for  hidden  Markov  models  (HMM)  in  [14]. 
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The  Kalman  filter,  the  extended  Kalman  filter  and  the  robust  filter  approaches  all  estimate 
state  information  such  as  mean  and  variance.  An  alternative  approach  is  to  consider  the 
evolution  of  the  probability  density  functions  (PDF)  directly.  The  integrals  describing  the 
evolution  of  these  probability  density  functions  generally  do  not  have  analytic  solutions 
(except  in  simple  cases  corresponding  to  the  Kalman  filter)  and  solutions  must  be  obtained 
through  numeric  methods.  One  interesting  numeric  approach  that  has  recently  appeared 
is  the  particle  filter  approach. 

In  the  particle  filter  approach  the  probability  density  function  of  the  system  state  is  ap¬ 
proximated  by  a  system  of  discrete  points  (or  particles)  which  evolve  according  to  the 
system  equations  [16].  At  any  time  point,  the  set  of  particles  can  be  used  to  approxi¬ 
mate  the  probability  density  function  of  the  state.  It  can  be  shown  that  as  the  number 
of  particle  increases  to  infinity,  the  approximation  approaches  the  true  probability  density 
function  [16].  These  particle  filter  approaches  have  the  advantage  that  they  work  for  any 
general  non-linear  system  description.  Closely  related  to  the  particle  filter  approach  are 
importance  sampling  approaches  [26]  and  Monte-Carlo  simulation  techniques  [27].  In  this 
report  we  present  the  particle  filter  approach  but  no  details  of  these  related  techniques  are 
provided. 

Another  technique  for  estimating  the  PDF  directly  is  via  an  HMM  approximation.  If  the 
state  space  is  naturally  bounded  then  it  is  possible  to  approximate  the  state  space  by  finite 
regions  and  use  the  powerful  signal  processing  tool  developed  for  hidden  Markov  models 
to  estimate  the  PDF.  An  outline  of  an  HMM  approximation  technique  is  given. 

This  report  is  organised  as  follows:  In  Section  2  the  Kalman  filtering  problem  is  presented 
and  the  optimal  solution  is  given.  In  Section  3  a  numerically  stable  form  of  the  Kalman 
filter  known  as  the  U-D  covariance  factorisation  of  the  Kalman  filter  is  presented.  The 
extended  Kalman  filter  along  with  stability  results  are  then  presented  in  Section  4.  In 
Section  5  some  robust  filtering  results  are  presented.  The  particle  filtering  approach  is 
represented  in  Section  6.  Some  details  of  hidden  Markov  model  approximations  are  then 
given.  Some  application  examples  are  presented  in  Section  7  which  compare  the  various 
algorithms  presented  in  this  report.  Finally,  in  Section  8  some  conclusions  are  presented. 
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2  Discrete-Time  Kalman  Filter  Problem 

This  section  introduces  a  model  for  a  discrete-time  linear  Gauss-Markov  system  and  the 
optimal  filter  for  the  state  variable.  This  optimal  filter  is  known  as  the  Kalman  filter. 

Consider  a  stochastic  process  xk  taking  values  in  RN .  Assume  that  the  dynamics  of  Xk 
are  described  for  k  E  Z+,  where  Z+  is  the  set  of  non-negative  integers,  by 

xk+i  =  Akxk  +  Bkwk  (2.1) 

where  Ak  E  R(NxN)  is  called  the  state  transition  matrix.  Bk  E  R(NxS^  and  wk  E  R(-Sx1'1 
is  a  sequence  of  independent  and  identically  distributed  ( iid)  N[0,Qk)  vector  random 
variables  called  the  process  noise.  The  matrix  Qk  is  a  non-negative  definite  symmetric 
matrix.  We  assume  that  xo  is  a  N(xo,Pq)  vector  random  variable.  For  simplicity  we 
assume  that  there  is  no  input  into  this  system  but  the  results  in  this  report  can  easily  be 
extended  to  a  system  with  a  measured  input  signal. 

Further  suppose  that  xk  is  observed  indirectly  via  the  vector  measurement  process  yk  E 
R(Mx i)  described  as  follows: 

Vk  =  Ckxk  +  vk  (2.2) 

where  Ck  E  r(m'xN')  is  the  called  the  output  matrix  and  vk  E  is  a  sequence  of 

iid  N(0,Rk)  scalar  random  variables  called  the  measurement  noise.  We  assume  that  Rk 
(which  is  a  non-negative  symmetric  matrix)  is  non-singular.  Let  yk  :=  {yo>  j/i,  -  •  • ,  yk} 
denote  the  measurements  up  to  time  k.  The  symbol  :=  denotes  “defined  as” . 

2.1  The  Filtering  Problem 

The  filtering  problem  stated  in  the  broadest  terms  is  to  determine  information  about 
the  state  xk  from  measurements  up  until  time  k  (or  k  —  1).  For  stochastic  processes, 
this  information  is  represented  by  the  conditional  probability  density  functions  (PDFs) 
p(xk\yk)  (or  p{xk\yk-i))-  Usually  we  limit  our  interest  to  statistics  of  the  state  such  as 
the  mean  and  variance  that  can  be  calculated  from  these  PDFs,  but  this  is  not  always  the 
case  (see  Section  6  later  in  this  report).  The  filtering  problem  then  becomes  the  problem 
of  estimating  these  particular  statistics. 
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For  the  special  case  of  the  linear  Gauss-Markov  system  with  Gaussian  initial  conditions 
it  can  be  shown  that  the  PDFs  at  each  time  instant  are  Gaussian  and  hence  the  PDFs 
can  be  completely  specified  by  the  mean  and  variance  statistics  (which  is  a  finite  number 
of  quantities).  For  this  special  case,  a  finite-dimensional  filter  (the  Kalman  filter)  can  be 
implemented  that  optimally  calculates  these  two  statistics  and  hence  completely  specifies 
the  PDFs  at  each  time  instant. 

In  general  terms,  the  linear  Gauss-Markov  filtering  problem  is  a  special  case  and  for  most 
other  filtering  problems  the  relevant  PDFs  can  not  be  represented  by  a  finite  number  of 
statistics  and  finite-dimensional  optimal  filters  are  not  possible. 

Let  us  now  concentrate  on  the  filtering  problem  associated  with  the  Kalman  filter.  Let  us 
define  the  conditional  mean  estimates  xk\k-i  :=  E[xk\yk^.i]  and  xk\k  :=  E[xk\yk}.  And  let 
us  also  define  the  error  covariance  matrices  Pk\k-\  E[(xk  -  xk\k-i)(xk  -  £fc|fc-i)'|34-i] 
and  Pk\k  • =  E[{xk  xk\k)(xk 

The  discrete-time  Kalman  filtering  problem  is  stated  as  follows:  For  the  linear  system 
(2.1) (2.2)  defined  for  k  >  0  for  which  the  initial  state  xq  is  a  Gaussian  random  variable 
with  mean  xq  and  covariance  Pq  which  are  independent  of  {u^}  and  {vk},  determine  the 
estimates  and  xk\k  and  the  associated  error  covariance  matrices  Pk\k-i  and  Pk\k. 

2.2  The  Kalman  Filter 

It  is  well  known  that  the  discrete-time  Kalman  filter  is  a  finite- dimensional  optimal  solution 
to  this  problem  [1,  2,  3].  Here  finite-dimensional  in  the  sense  that  it  can  be  implemented 
exactly  with  a  finite  number  of  operations  and  a  finite  amount  of  memory  and  optimality 
is  in  a  conditional  mean  sense.  The  optimality  of  the  Kalman  filter  can  be  established  in 
a  variety  of  ways  and  we  refer  the  reader  to  [1]  for  a  good  presentation.  In  this  report  we 
simply  present  the  Kalman  filter  without  proof. 

The  standard  implementation  of  the  Kalman  filter  is  the  following  recursion. 

%k\k-l  =  Ak-\Xk-i\k-\ 

Pk\k-i  =  M-\Pk-i\k-\Ak-\  +  Bk_iQkB'k_1 
Kk  =  Pk\k-\c'k  ^kPk\k-\C'k  +-R*] 

%k\ k  —  %k\k-l  +  Kk  [yfc  -  CkX A;|fc_i] 


5 


DSTO-TR-1301 


Pk\k  ~  Pk\k-l  ~  KkCkPk\k-l  (2-3) 

where  x0|o  =  E[x 0]  =  £0  and  P0|o  =  E[(xq  -  x0){x0  -  so)']  =  Po-  The  recursion  (2.3)  is  one 
of  the  many  algebraically  equivalent  recursions  for  the  same  estimates  and  Xk\k- 

It  is  common  to  interpret  each  iteration  of  the  Kalman  filter  equations  as  two  steps. 
The  first  two  equations  in  (2.3)  correspond  to  a  time  update  process  and  the  final  three 
equations  as  a  measurement  update  process.  In  the  time  update  stage,  the  previous 
estimate  is  used  to  predict  the  state  value  (and  the  covariance)  at  the  next  time  instant. 
In  the  measurement  update  stage,  the  prediction  of  the  state  (and  the  covariance)  is 
corrected  using  the  information  in  the  new  measurement. 

Alternatively,  the  Kalman  filtering  problem  can  be  interpreted  as  designing  Kk  such  that 
the  filter  error  is  white  (in  particular  uncorrelated),  see  Figure  1.  To  design  Kk,  knowledge 
of  Ak,  Ck,  Qk  and  Rk  is  required.  If  these  quantities  are  not  known  with  complete  certainty 
then  the  conditional  mean  optimal  filter  can  not  be  designed. 


Figure  1  (U):  Block  diagram  for  state  estimate  update  as  a  whitening  filtering 


Remarks 

1.  It  can  be  shown  that  the  Kalman  filter:  is  the  minimum  variance  filter  [1,  8],  is 
unbiased  [1],  and  is  a  recursive  Bayesian  estimator  optimal  in  a  minimum  mean 
square  sense.  In  addition,  if  the  Gaussianity  assumptions  on  xo,  {wk}  arid  {vk}  no 
longer  hold  then  the  Kalman  filter  is  the  minimum  variance  filter  among  the  class 
of  linear  filters  [1,  8]. 
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2.  Under  reasonable  conditions  (see  [1,  8])  the  Kalman  filter  is  asymptotically  stable 
in  the  sense  that  it  will  exponentially  forget  any  initial  condition  errors.  This  is  an 
important  property  because  it  means  that  errors  introduced  during  filtering  do  not 
necessarily  make  the  Kalman  filter  diverge. 

3.  The  recursion  (2.3)  is  the  standard  form  of  the  Kalman  Filter.  Another  common 
form  of  the  Kalman  filter  is  the  innovations  form  [1].  There  are  many  other  forms  of 
the  filter  that  include  slight  modifications  of  the  covariance  and  gain  equations  and 
in  the  next  section  we  consider  one  form  that  has  improved  numeric  stability. 

3  U-D  Factorisation  Form  of  the  Kalman  Filter 

3.1  Filter  Stability 

According  to  Maybeck  [2]:  “An  algorithm  can  be  said  to  be  numerically  stable  if  the 
computed  result  of  the  algorithm  corresponds  to  an  exactly  computed  solution  to  a  problem 
that  is  only  slightly  perturbed  from  the  original  one.”  The  standard  formulation  of  the 
Kalman  filter  is  not  always  numerically  stable  in  this  sense  [4], 

There  can  be  numerical  problems  in  implementing  the  above  Kalman  filter  recursions  on 
a  finite  word-length  digital  processor.  For  example,  although  it  is  theoretically  impossible 
for  the  covariance  matrices  to  have  negative  eigenvalues,  such  a  situation  can  arise  due 
to  inaccuracies  in  numerical  calculations.  Such  a  condition  can  lead  to  instability  or 
divergence  of  the  Kalman  filter. 

To  avoid  the  numerical  problems  of  the  standard  formulation  of  the  Kalman  filter  equa¬ 
tions,  many  alternative  recursions  have  been  developed  [2,  1].  These  alternative  recursions 
update  the  state  and  covariance  matrices  in  a  way  that  ensures  that  numerical  problems 
are  avoided.  These  formulations  are  algebraically  equivalent  to  the  standard  Kalman  filter 
equations  but  exhibit  improved  numerical  precision  and  stability  [2].  Because  these  alter¬ 
native  forms  are  algebraically  equivalent,  the  design  and  tuning  of  the  optimal  filter  can 
be  done  using  the  standard  equations,  ignoring  errors  caused  by  numerical  instability,  and 
the  numerically  stable  forms  only  need  be  considered  when  the  actual  implementation  is 
performed  [2]. 
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It  should  be  noted  that  the  sort  of  numerical  instabilities  inherent  in  the  standard  Kalman 
filter  equations  are  issues  that  appear  when  some  of  the  measurements  are  very  accurate 
[1],  when  the  process  covariance  has  large  dynamic  range  [7]  or  when  the  Kalman  filter 
is  applied  to  non-linear  problems  through  a  problem  formulation  such  as  the  extended 
Kalman  filter.  In  general  terms,  the  numerical  precision  of  modern  computational  tech¬ 
nologies  has  reduced  the  need  to  use  numerically  stable  forms  of  the  Kalman  filter. 

There  are  two  major  advantages  in  using  one  of  the  numerical  stable  forms  of  the  Kalman 
filter  [1]: 

1.  The  recursions  can  be  formulated  in  a  way  that  ensure  that  computational  errors 
cannot  lead  to  a  matrix  (Pk\k  or  Pk\k-i)  that  fails  to  be  nonnegative  definite  or 
symmetric. 

2.  The  numerical  conditioning  of  the  recursions  can  be  improved  so  that  only  half  as 
much  precision  is  required  in  the  computations. 

The  approach  presented  here  to  enhance  the  numerical  characteristics  of  the  Kalman 
filter  is  known  as  the  “U-D  covariance  factorisation”  developed  by  Bierman  and  Thornton 
[4,  5,  6].  There  are  several  other  forms  of  the  Kalman  filter  with  increased  numeric 
stability,  such  as  the  square  root  forms,  see  [2,  1]  for  details.  We  consider  only  the  U-D 
factorisation  in  this  report  because  it  offers  a  reasonable  compromise  of  numerical  stability 
and  increased  computational  effort  [2]. 

3.2  The  U-D  Factorisation  of  the  Kalman  filter 

The  U-D  factorisation  involves  expressing  the  error  covariance  matrices  as 

Pk\k-i  =  U(k\k  —  l)D(k\k  —  l)U{k\k  —  1)'  and 
Pk\k  =  U{k\k)D(k\k)U{k\k)'  (3.1) 

where  U  are  unitary  upper  triangular  matrices  and  D  are  diagonal.  The  above  U-D 
factorisation  always  exists  for  square  symmetric,  positive  semi-definite  matrices  [2];  hence, 
the  error  covariance  matrices  Pk\k  and  Pk\k-i  can  always  be  factorised  this  way. 


8 


DSTO-TR-1301 


Although  the  U-D  factorisation  is  not  unique  we  can  work  with  the  following  factorisation 
for  initialisation  purposes:  For  a  P  (either  Pk\k-i  or  Pk\k)  j 

N 

Djj  =  Pjj  —  ^2  DifUjf  and 
e=j+ 1 


Uij  =  1 


(Pij  ~  £*"  +i  DuUitUjti/Djj  i  =  j  -  1,  j  -  2, . . . ,  1 


where  TJtj  is  the  ij  th  element  of  U  etc.  and 


Dnn  =  Pnn  and 


UiN  — 


i  =  N 


Pin/Dnn  i  =  N  —  1,  N  —  2. . . . ,  1. 


To  develop  the  filter  itself  we  consider  the  time  update  and  measurement  update  separately 
in  the  following  subsections. 

3.3  Time  Update 

It  can  be  shown  that  the  time  update  equations  for  the  U-D  factorisation  of  the  Kalman 
filter  can  be  implemented  as  follows  (taken  from  [2]).  Using  the  following  definitions: 


Yk  :=  [Ak-iU^-^-^Bk-i], 

"  0 

Dk+l  ''=  [  0  Qk-x  J  ’ 

[ai|a2|  -  •  •  |  a  at]  :=  Yk  and 


r(fc|fc-i) 


1  for  j  =  1, . . . ,  N 


where  0  are  zero  matrices.  D^k~l lfc_1)  and  denote  the  U-D  factorisation  of 

Pk-x\k-i-  Yk  is  an  (TV  x  (TV  +  S ))  matrix,  £>k+x  is  an  (TV  +  S'  x  TV  +  5)  matrix  and 
at  are  2TV  vectors.  Then  the  time  update  equations  can  be  calculated  recursively  for 
t  =  TV.TV- 1,...,1: 


-,(k\k—  1) 

'te 

de 

7-(/c|fc—  1) 


{k\k-l) 


TT{k\k—l) 

dj-Uji  a£ 


3  =  1,2, 
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where  D^k~^  and  U^k  H  denote  the  U-D  factorisation  of  Pk\k-i-  Here  at,  ce  and  de  are 
temporary  variables  that  are  reused.  The  state  estimate  is  updated  as  follows: 

%k\k-l  —  Ak-lXk-l\k-l-  (3-6) 

3.4  Measurement  Update 

The  scalar  measurement  update  equations  for  the  U-D  factorisation  form  of  the  Kalman 
filter  can  calculated  using  the  following  definitions: 

/  :=  uWk-l)'C'k 

Vj  ~  D^k-l)fj  for  j  =  l,2,...,N 

ao  =  R  and  (3.7) 

U\fk)  :=  1  for  j  —  1,2, ...  ,N.  (3.8) 

Then  the  time  update  equations  can  be  calculated  recursively  for  i  =  1, . . .  ,N: 

at  —  at- 1  4-  ftvt 
Dee(k\k )  =  Da{k\k —  l)at-i/at 
be  =  ve 

Vi  =  —f el  at- 1 

Ujt{k\k)  =  Ujt(k\k  -1)  +  bjPt  1  ^  (3.9) 

bj  =  bj  +  Ujt{k\k  —  1  )vt  J 

where  ak,  bk  and  pt-  are  temporary  variables  that  are  reused.  Vector  measurement  updates 
can  be  handled  as  a  number  of  scalar  measurement  updates. 

Let  b  denote  the  vector  formed  from  elements  be ,  then  the  state  is  updated  as  follows: 

Kk  =  b/att 

%k\k  =  £fc|fc-l  +  Kk{Vk  -  CkXk\k-l)-  (3.10) 

3.5  Computational  Effort 
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computational  effort  required  to  implement  the  U-D  factorisation  is  slightly  greater  than 
the  standard  form.  However,  considering  the  requirement  for  numerical  stability,  the  U-D 
form  of  the  algorithm  appears  to  offer  a  reasonable  compromise  between  computational 
efficiency  and  numerical  stability.  See  [2]  for  more  details  and  a  description  of  the  actually 
computational  effort  required. 

In  most  situations  the  standard  Kalman  filter  can  be  made  to  work  by  increasing  the 
precision  of  computations  as  required  or  using  ad  hoc  modifications  to  reduce  the  dynamic 
range  of  variables  [1],  However,  in  these  situations,  better  performance  can  often  be 
achieved  by  the  U-D  form  of  the  Kalman  filter  using  less  precision  in  the  computations. 
For  these  reasons,  a  number  of  practitioners  have  argued  that  numerically  stable  forms  of 
the  Kalman  filter  should  always  be  used  in  preference  to  the  standard  form  of  the  Kalman 
filter,  rather  than  switching  to  a  stable  form  when  required  [2]. 

Finally,  it  should  be  noted  that  in  high  measurement  noise  situations  the  standard  Kalman 
filtering  equations  tend  to  be  fairly  stable,  reducing  the  motivation  for  numerically  stable 
forms.  In  fact,  in  some  high  measurement  noise  situations,  the  standard  Kalman  filter 
equations  can  be  simplified  though  approximations  that  considerably  reduce  computa¬ 
tional  burden  with  little  loss  in  optimality  and  without  introducing  stability  problems 
[!]■ 


4  Extended  Kalman  Filter 

The  discussion  so  far  in  this  report  has  been  directed  towards  optimal  filtering  of  linear 
systems  using  the  Kalman  filter.  However,  in  most  realistic  applications  the  underly¬ 
ing  physical  system  is  non-linear.  In  some  situations,  slightly  non-linear  systems  can  be 
approximated  as  linear  systems  and  the  Kalman  filter  provides  a  satisfactory  filtering  so¬ 
lution.  In  other  situations,  the  system  may  have  obvious  non-linear  characteristics  that 
can  not  be  ignored. 

Filtering  for  non-linear  systems  is  a  difficult  problem  for  which  few  satisfactory  solutions 
can  be  found  (we  consider  several  algorithms  in  this  report  that  are  somewhat  satisfactory 
in  some  situations).  The  sub-optimal  approach  considered  in  this  section  is  an  extension 
of  the  Kalman  filter  known  as  the  extended  Kalman  filter. 
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The  extended  Kalman  filter  is  posed  by  linearisation  of  a  non-linear  system.  Consider  the 
following  non-linear  system  defined  for  k  £  Z+: 

xk+ 1  =  ak(xk)  "b  ^k{xk)wk 

Vk  =  Ck(xk)+vk  (4.1) 


where  ak(.),  bk(.)  and  ck(.)  are  non-hnear  functions  of  the  state  and  wk.vk  are  defined  as 
before.  Let  us  define  the  following  quantities: 


Ak  = 


dak(x) 

dx 


x~xk\k  —  1 


Bk  = 


dbk(x) 

dx 


and 


Ck  = 


dck(x ) 
dx 


x~xk\k~  1 


(4.2) 


Let  us  also  introduce  matrices  Qk  and  R*k  which  are  related  to  the  covariance  matrices  for 
noises  wk  and  vk.  However,  as  will  be  shown  later  in  Section  4.2,  the  matrices  Q*k  and  R*k 
need  not  equal  Qk  and  Rk  and  other  positive  definite  matrices  are  often  better  choices. 


The  extended  Kalman  filter  is  implemented  using  the  following  equations: 


—  ak— l(xk—  l|fc— l) 

Pk\k-\  —  Ak-iPk-i\k-iA'k~i  +  Bk^iQlBk_1 

Kk  =  P^k-iC'klCkPk^C'k  +  Rty1 

xk\k  =  xk\k—\  "b  P-k  —  cfc(^fc|A:-l)j 
Pk\k  =  Pk\k-l  ~  KkCkPk\k-\.  (4.3) 

The  equations  are  no  longer  optimal  or  linear  because  Ak  etc.  depend  on  xk\k-i  etc.  The 
symbols  x^-i,  Pk\k-i  and  -^fc — 1 1 fc — i  now  loosely  denote  approximate  conditional 

means  and  covariances  respectively. 

The  extended  Kalman  filter  presented  above  is  based  on  first  order  linearisation  of  a  non¬ 
linear  system,  but  there  are  many  variations  on  the  extended  Kalman  filter  based  on 
second  order  linearisation  or  iterative  techniques.  Although  the  extended  Kalman  filter  or 
other  linearisation  techniques  are  no  longer  optimal,  these  filters  can  provide  reasonable 
filtering  performance  in  some  situations. 


4.1  U-D  Covariance  Form  of  the  Extended  Kalman  Filter 

A  U-D  factorisation  form  of  the  extended  Kalman  filter  can  be  posed  by  appropriate 
modification  of  U-D  factorisation  of  the  Kalman  filter  as  follows. 
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Equations  (3.10)  and  (3.6)  are  replaced  by  the  following  equations: 

=  ak— 1  i^k—  l|fc — 1 ) 

Xk\k  —  *£/fc|fc  —  1  d"  Kk(yk  Ck{%k\k—  l))’  (^‘^) 

4.2  Stochastic  Stability  of  the  Extended  Kalman  Filter 

A  key  question  when  applying  an  extended  Kalman  filter  to  a  particular  non-linear  problem 
is  when  will  the  extended  Kalman  filter  be  stable  and  when  will  it  diverge?  Heuristic 
arguments  have  been  used  to  suggest  that  if  the  non-linearities  are  linear  enough  and  the 
filter  is  initialised  well  enough  then  the  filter  should  be  stable.  This  heuristic  argument  has 
encouraged  the  use  of  the  extended  Kalman  filter  in  a  wide  variety  of  signal  processing, 
control  and  filtering  problems.  However,  without  any  solid  stability  results,  the  error 
behaviour  of  the  extended  Kalman  filter  needs  to  be  examined  through  testing  whenever 
applied  [8,  1]. 

Recently,  solid  stability  results  have  established  conditions  on  the  non-linearities  and  ini¬ 
tial  conditions  which  ensure  that  the  extended  Kalman  filter  will  produce  estimates  with 
bounded  error  [9,  10].  These  results  answer  some  of  the  stability  questions  surrounding 
the  extended  Kalman  filter  [9,  10].  This  section  repeats  the  stability  results  of  [10]. 

Consider  again  the  non-linear  system  (4.1)  defined  in  Section  4: 

zfc+i  =  ak(xk)  +  bk(xk)wk 

Vk  =  ck{xk)+vk.  (4.5) 

Let  us  define  the  following  quantities 

tp{x k:  Xk)  ak{xk)  ak(xk)  Ak(xk  Xk) 

x(xk,xk)  :=  ck(xk)  -  ck(xk)  -  Ck{ Xk-Xk) 

where  xk  is  some  estimate  of  the  state  (see  Figure  2  for  a  graphic  interpretation  of 
tp(xk,xk)).  Also,  we  define  the  estimation  error  as  xk\k  :=  xk  —  xk\k.  Then  the  following 
theorem  is  presented  in  [10]. 

Theorem  1  (Theorem  3.1)  Consider  the  nonlinear  system  (4-1)  and  the  extended  Kalman 
filter  presented  in  Section  4 ■  Let  the  following  hold: 


13 


DSTO-TR-1301 


Figure  2  (U):  Graphical  interpretation  of  tp(xk,Xk). 

1.  There  are  positive  real  numbers  a,  c,p,p,  q,r>  0  such  that  the  following  bounds  hold 
for  all  k  >  0: 

11-4*11  <  a 

\\Ck\\  <  c 
pi  <  Pk\k-i<PI 
Q  <  Qk 

L  <  R*k-  (4.6) 


2.  Ak  is  nonsingular  for  all  k  >  0. 


3.  There  are  positive  numbers  ex.  kx  >  0  such  that 

\\q>{xk.,xk)\\  <  «v»||«*  ~Xk\? 

Ibc(s*i®*)ll  <  Kx\\xk  -Xk\?  (4-7) 

for  Xk,  xk  with  Ha*  —  Xk\\  <  and  | la:*  —  xk\\  <  ex  respectively  for  all  k  where  xk 
is  any  estimate  of  xk  at  time  k. 
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Then  the  estimation  error  is  bounded  with  probability  one,  provided  the  initial  estimation 
error  satisfies 

||x0||<e  (4.8) 

and  the  covariances  of  the  noise  terms  are  bounded  via  Qk  <  SI  and  J4  <  SI  for  some 
5,  e. 


Remark 


4.  The  proof  of  Theorem  1  is  given  in  [10]. 

5.  This  result  states  that  if  the  non-linearity  is  small  then  the  EKF  is  stable  if  initialised 
close  enough  to  the  true  initial  value.  The  greater  the  deviation  from  linearity  the 
better  the  initialisation  needs  to  be. 


The  proof  presented  in  [10]  provides  a  technique  for  calculating  conservative  bounds  for 
e  and  <5.  Simulation  studies  suggest  that  e  and  8  can  be  significantly  larger  than  these 
bounds  in  some  situations. 


We  define  the  following  to  repeat  the  bounds  presented  in  [10] 


We  also  define  the  following 
Knonl 

Knoise 

a 

Then 


and 


e  :=  mm(  e^.e 


ip- 


k  :=  Kp+ap-K,x. 


K  (J  C2  ,  __ 

—  2  o  +  ap—  +  Ke 

p\\  r. 

S  a2c2p2 


- h 

P 


pr_ 


:=  l-l/(l  +  - 

\  pa 


pa2(  1  +pc2/r) 


e  =  mm  e 


2 P&nonl  J 


5  = 


ae 


2  psinois 


(4.9) 

(4.10) 

(4.11) 

(4.12) 

(4.13) 

(4.14) 

(4.15) 


To  gain  an  understanding  of  the  stability  of  the  extended  Kalman  filter  consider  the 
following  special  case. 
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Example:  Quadratic  Non-linearities 

Consider  the  situation  where  system  functions,  ajt(.)  and  c*,(.)  are  quadratic  in  the  state. 
Then  < p  and  x  are  bounded  in  (4.7)  for  all  and  ex.  Hence  Knoni  is  unbounded  and 
hence  e  is  unbounded.  The  conclusion  is  that  if  the  non-linear  elements  in  the  model  axe 
quadratic  then  the  extended  Kalman  filter  is  stable  for  any  initial  estimate. 

4.3  Higher-Order  Approximation  Methods  and  Other  Is¬ 
sues 

The  extended  Kalman  filter  presented  above  is  based  on  a  first-order  Taylor  series  approx¬ 
imation  of  the  system  non-linearity.  There  are  a  number  of  variations  and  generalisation 
of  the  ideas  used  in  the  extended  Kalman  filter  [1,  p.  196].  These  ideas  include  second  (and 
higher  order)  Taylor  series  approximation  of  the  non-linear  system,  banks  of  extended 
Kalman  filters  (or  the  Gaussian  sum  approach),  Monte-Carlo  simulation  techniques  [27] 
(related  to  the  particle  filter  ideas  presented  in  Section  6),  higher-order  moment  filters, 
and  others. 

In  general  terms,  any  one  of  these  filtering  approaches  may  be  superior  to  the  extended 
Kalman  filter  in  a  particular  situation,  but  none  of  these  approaches  is  superior  in  all  ap¬ 
plications.  The  particular  type  of  non-linearities  present  in  a  system  may  make  particular 
approaches  more  fruitful  than  others,  but  there  are  no  real  guidelines.  Some  approaches, 
such  as  Monte-Carlo  techniques,  may  appear  to  be  suitable  to  a  large  class  of  problems; 
however,  their  general  applicability  is  at  the  cost  of  elegance  and  computational  load. 

A  reasonable  approach  (based  on  the  previous  experiences  of  the  author)  maybe  to  use 
the  simplest  feasible  filtering  approach  and  then  re-evaluate  the  choice  of  filter  if  the 
desired  performance  is  not  achieved.  It  is  likely  that  the  computational  requirements  of 
one  of  the  more  complicated  approximate  filtering  techniques  is  sufficient  motivation  to 
first  investigate  a  simpler  approach. 

One  possible  trap  to  avoid  is  the  posing  of  unrealistic  filtering  problems.  It  is  easy  to  pose 
filtering  problems  that  are  not  solvable  due  to  observability  problems.  In  other  situations, 
reasonable  filtering  performance  can  be  difficult  to  obtain  because  the  observed  system  is 
close  to  unobservable.  Observability  is  a  control  system’s  concept  that  is  defined  as  the 
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ability  to  determine  the  state  from  observations.  For  linear  systems  there  are  easy  tests 
for  observability  (see  [1,  29]  or  an  undergraduate  control  systems  text)  but  questions  of 
observability  axe  more  complicated  in  non-linear  systems  [28]. 

The  observability  condition  is  only  one  part  of  the  question,  because  a  system  is  either 
observable  or  not.  A  practitioner  should  also  be  interested  in  the  relative  difficulty  of  a 
filtering  problem.  One  way  of  quantifying  the  difficulty  of  achieving  good  state  estimates 
in  a  particular  filtering  problem  is  via  the  Fisher  information  matrix  and  the  Cramer- Rao 
lower  bound  [31,  30]. 

The  Fisher  information  matrix  describes  how  much  information  about  the  state  variable  is 
available  in  the  measurements.  The  Cramer-Rao  bound  is  a  lower  bound  on  the  amount  of 
data  required  in  a  particular  problem  to  achieve  a  particular  amount  of  certainty  in  esti¬ 
mation.  A  Fisher  information  matrix  with  poor  characteristics  highlights  that  a  particular 
problem  is  difficult  in  a  fundamental  way  (and  changing  the  filtering  algorithm  will  prob¬ 
ably  not  significantly  improve  filtering  performance).  When  a  system  is  not  observable  or 
the  Fisher  information  matrix  for  the  problem  has  poor  characteristics,  the  practitioner 
will  probably  need  to  consider  redesigning  the  system. 

The  task  of  choosing  an  appropriate  model  and  filtering  algorithm  for  a  non-linear  system 
is  non-trivial.  It  is  implicit  in  the  preceding  discussion  that  the  system  model  must  reflect 
the  nature  of  the  true  system.  This  report  does  not  address  any  of  the  approaches  (neither 
data  based  nor  first  principles  based)  for  designing  models  of  systems,  see  [30]  for  an 
introduction.  Once  a  system  model  has  been  obtained,  the  observability  of  this  system 
should  be  tested  and  the  Fisher  information  matrix  examined  to  determine  the  difficulty 
of  the  filtering  problem. 

Once  comfortable  that  the  measurements  have  enough  information,  a  filtering  algorithm 
can  be  chosen.  It  is  difficult  to  give  general  guidelines  for  algorithm  choice  but  experience 
with  similar  problems  can  provide  useful  insights.  A  practitioner  will  probably  have  a  bias 
towards  their  favorite  algorithm,  or  an  algorithm  that  worked  on  a  similar  problem,  and 
may  try  this  algorithm  first. 

Quantifying  the  achieved  performance  of  an  algorithm  can  also  be  difficult.  Comparison 
with  similar  problems  can  both  be  helpful  and  misleading.  A  reasonable  approach  would 
be  to  compare  the  performance  of  the  algorithm  with  the  performance  of  an  higher-order 
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algorithm.  This  comparison  may  give  a  feel  for  the  trade-off  between  complexity  and 
filtering  performance  for  this  problem  and  what  sort  of  algorithm  will  give  the  desired 
filtering  performance. 

In  the  next  section  we  consider  the  filtering  when  the  system  model  is  not  known  with 
complete  certainty. 


5  Robust  Filtering 

The  Kalman  filter  has  been  applied  to  a  large  class  of  problems.  However,  in  many 
situations  the  system  model  is  non-linear  or  uncertain  in  the  sense  that  the  model  is  not 
completely  known.  The  previous  section  dealt  with  use  of  the  extended  Kalman  filter  on  a 
linearised  version  of  a  non-linear  system.  Even  in  situations  where  the  non-linear  system  is 
completely  known  the  extended  Kalman  filter  may  give  poor  performance.  In  a  situation 
where  there  is  model  uncertainty,  it  can  be  shown  that  the  Kalman  filter  and  the  extended 
Kalman  filter  are  often  very  poor  filtering  choices  [24]. 

In  recent  years,  a  number  of  new  approaches  have  been  proposed  for  the  filtering  prob¬ 
lem  that  are  related  to  robust  control  techniques.  This  includes  approaches  such  as  H°° 
filtering  [25],  risk-sensitive  filtering  [11,  12]  and  robust  filtering  [24], 

5.1  The  Robust  Kalman  Filter 

In  this  section  we  describe  the  development  of  the  robust  Kalman  filter  for  uncertain 
discrete- time  systems  (this  development  is  given  in  [24]).  The  description  given  here  is  in 
terms  of  a  uncertain  systems  where  the  uncertainty  has  a  particular  form.  However,  the 
results  can  be  used  for  fairly  general  systems  by  transforming  the  uncertain  system  into 
the  following  form. 

Consider  the  following  time- varying  uncertain  discrete-time  system  defined  for  k  €  Z+: 

Zjfc+i  =  Akxk  + 

Zk  —  KkXk, 

Vk  =  Ckxk  +  vk 
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where  again  xk  is  the  state,  wk  G  RP  and  vk  G  Re  are  uncertainty  inputs,  zk  G  Rq  is  the 
un certainty  output,  yk  G  Re  is  the  measured  output,  and  Ak,  Blk,  Kk  and  Ck  are  given 
matrices  such  that  Ak  is  non-singular. 

The  uncertainty  in  this  system  is  constrained  to  satisfy  the  following  constraint  for  all  k. 

Let  N  -  N'  >  0  be  a  given  matrix,  x0  G  Rn  be  a  given  vector,  d<  0  be  a  given  constant, 
and  Qk  and  Rk  be  given  positive-definite  symmetric  matrices.  The  following  constraint  is 
termed  the  sum  quadratic  constraint: 

fc-l  k- 1 

(x'o  —  xo)'N(xo  —  zo)  +  y: (i u'jQeWj  +  v'l+lRi+ i«£+i)  <  d- fi  ^  ||^+i||2  for  all  k.  (5.2) 
e=o  e=o 

A  system  (5.1)  satisfying  the  sum  quadratic  constraint  can  be  thought  of  as  a  linear  system 
driven  by  a  process  noise  and  observed  in  a  measurement  noise  which  can  be  anywhere 
in  a  constrained  set.  This  description  of  an  uncertain  system  is  fairly  general  because 
uncertainties  in  system  matrices  such  as  A  can  be  thought  of  as  noise  terms. 

Before  proceeding  to  present  filtering  results,  we  give  an  example  of  a  structured  uncer¬ 
tainty  system  that  satisfies  (5.1)  and  (5.2).  Consider  a  system  where  there  is  some  uncer¬ 
tainty  in  the  state  transition  matrix.  The  uncertainty  could  result  because  the  dynamics  of 
the  physical  system  can  not  be  determined  but  these  dynamics  need  to  be  modelled  math¬ 
ematically  in  some  way.  This  uncertainty  may  be  represented  by  the  following  equations 
(tills  example  is  given  in  [24]): 

Xk+i  —  [Ak  +  B}?’ &kKk\xk  +  Bkln  k, 

Vk  =  Ckxk+nk,  with  ||A'fc(5|||  <  1  (5.3) 

where  Ak  is  the  uncertainty  matrix,  nk  and  nk  are  noise  sequences,  Bk  =  [Bj^.Bjf],  and 
||. 1 1  denotes  the  standard  induced  matrix  norm.  The  system  uncertainty  is  in  the  state 
transition  matrix  because  the  value  of  Ak  is  not  known.  Also,  let  this  system  satisfy  the 
condition 

fc-l  k 

(xq  -  xo)'N(xq  -  X’o)  +  XqKqQqKqxo  +  y 1(nt)'Qtrik  +  ^{nk)' R-k^k  <  d. 

£=0  £—0 

To  establish  that  (5.3)  is  admissible  for  the  uncertain  system  described  by  (5.1), (5. 2).  let 

AkKkXk 

Wk  = 

nk 


DSTO-TR-1301 


and  vk  =  nk  for  all  k  where 

W*kQh\<i 

for  all  k.  Then  condition  (5.2)  is  satisfied.  Hence  system  (5.3)  satisfies  the  requirements  of 
the  theory  and  the  results  developed  in  the  following  section  can  be  applied  to  this  system. 

5.2  The  Set- value  State  Estimation  Problem 

The  filtering  problem  examined  in  this  section  can  be  stated  as  follows:  given  an  output 
sequence  {yo,  yu  ■  ■  ■ ,  2/*}  then  find  the  corresponding  set  of  all  possible  states  xk  at  time 
k  with  uncertainty  inputs  and  initial  conditions  satisfying  the  constraint  (5.2). 

Definition  1  The  system  (5.1),  (5.2)  is  said  to  be  strictly  verifiable  if  the  set  of  possible 
states  Xk  at  time  k  is  bounded  for  any  xq,  {yk}  and  d. 

Definition  2  The  output  sequence  {yo?  •  •  • ,  Vk}  realizable  if  there  exist  sequences  {xk}, 
{wk}  and  {tifc}  satisfying  (5.1)  and  (5.2). 

In  addition  to  solving  the  state  estimation  problem,  the  results  that  follow  also  solve  the 
following  problem:  given  an  output  sequence,  determine  if  this  output  is  realizable  for  the 
un certainty  system  [24].  Thus,  the  following  results  are  useful  in  answering  questions  of 
model  validation  [24]. 

The  solution  to  the  filtering  problem  involves  the  following  Riccati  difference  equation: 

Fk+l  =  [B'kSkBk  +  Qk]*B'kSkAk, 

Sk+\  =  A'kSk[Ak  -  BkFk+i]  +  C'k+lRk+iCk+i  -  K'k+1Kk+1 

So  —  N  (5.4) 

where 

Ak  '■=  Akl,  Bk  :=  AkBk 

and  [.]#  denotes  the  Moore-Penrose  pseudo-inverse  (see  [1])  if  an  inverse  does  not  exist. 
Solutions  to  the  Riccati  equations  are  required  to  satisfy  the  following  conditions: 

B'kSkBk  +Qk  >  0  and 

M{B'kSkBk  +  Qk)  C  N{A'kSkBk)  (5.5) 
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for  all  k.  Here  A f  denotes  the  operation  of  taking  the  null  space  of  a  matrix. 

The  state  estimate  solution  requires  the  following  additional  equations  [24]. 

Vk+i  ~  [-A*  ~  BkFk+\)'rjk  +  C'k+}Rk+iyk+u 

Vo  =  Nx0, 

9k+\  =  9k  +  y'k+iRk+m+i  -  Vi [Bk[B'kSkBk  +  Qk]#Bkr]k, 

g0  =  XqNx0.  (5-6) 

Theorem  2  [24]  Consider  the  uncertainty  system  (5.1), (5. 2).  Then  the  following  state¬ 
ments  hold: 

(i)  The  uncertainty  system  (5.1), (5. 2)  is  strictly  verifiable  if  and  only  if  there  exists  a 
solution  to  the  Riccati  equation  (5-4)  satisfying  condition  (5.5). 

(ii)  Suppose  the  uncertainty  system  (5.1), (5. 2)  is  strictly  verifiable.  Then  the  output 
sequence  {yk}  is  realizable  if  and  only  if  pk{{yk})  >  —d  where 

Pk{{yk})  ■■=  v'kSkXVk-9k- 


(in)  Suppose  the  uncertainty  system  (5.1),  (5. 2)  is  strictly  verifiable,  then  the  set  of  pos¬ 
sible  state  values  at  time  k  is 


xk  G  Rn 


^xk 


Sk  2Vk 


<Pk{{yk})  +  d 


(5.7) 


Proof:  See  [24.  pp.  75-77]. 

The  center  of  the  solution  set  (5.7)  can  be  used  as  a  state  estimate  and  hence  it  follows 
that  the  state  estimate  at  time  k  is  xk  =  Sfilrjk  (see  [24]). 

To  demonstrate  the  performance  of  the  filter,  consider  the  following  example. 
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5.3  An  Example  Robust  Kalman  Filtering  Problem 


This  example  was  given  in  [24]  to  illustrate  the  robust  filter.  Consider  the  following 
structured  uncertainty  system,  ie.  the  form  of  (5.3). 


®fc+l(l) 

‘  1.98 +  0.0127 A*  -1  0 

a*(l) 

'  0.707  ‘ 

*fc+i(2) 

= 

1 

0  0 

xk(  2) 

+ 

0 

Xk+ 1(3)  _ 

0.4 

0  0.2 

.  ^(3)  . 

0 

Vk  =  Zfc(3)  +  nk  (5.8) 

where  zo  =  [000]', 

(10  +  0.01272) | |ar0 1 12  +  £(n,)2  +  £( ne )2  <  1 

£=0  £=1 

and  the  uncertainty  parameter  Ak  satisfies 


I|A*||<1 


for  all  k.  Note  that  even  if  A*  =  0  this  filtering  problem  does  not  reduce  to  the  linear- 
Gauss  filtering  problem  solved  by  the  Kalman  filter  because  the  noise  terms  satisfy  the 
above  sum  quadratic  constraint  rather  than  having  Gaussian  density  functions. 

In  this  problem,  the  state  equation  is  known  with  some,  but  not  complete,  certainty.  The 
variable  Ak  parameterises  the  possible  values  of  the  state  transition  matrix.  We  know 
only  that  this  parameter  is  constrained  to  a  particular  range. 

To  apply  the  results  of  Theorem  2  to  this  filtering  problem,  we  consider  the  system  in  the 
uncertainty  form  (5.1).  In  this  case,  the  matrices  A.  N,  B1,  xq,  K ,  C,  Q.  and  R  are  given 

by 


'  1.98 

-1 

0 

'  10 

0 

0 

'  0.707  0.707  " 

'  0  ' 

A  = 

1 

0 

0 

,N  = 

0 

10 

0 

,Bl  = 

0  0 

,Xq  = 

0 

0.4 

0 

0.2 

0 

0 

10 

0  0 

0 

0  0 

K 

=  [  0.0127 

0  0 

]. 

Q  —  I  and  J?  =  1. 

(5-J 

The  constant  d  is  given  by  d  =  1. 


To  illustrate  the  performance  of  the  robust  filter,  the  system  was  subjected  to  the  following 
noise  sequences: 


f  0.5  for  k  —  0, 
|  0  otherwise 


and  nk  =  —  sin(A;/10). 
1U5 
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It  is  straightforward  to  verify  that  the  uncertainty  input  sequences 
wk  =  [  a :'kK'A'  { nk )'  ]'  and  vk  =  nk 
satisfy  the  sum  quadratic  constraint  (5.2). 

Figure  3  shows  estimates  of  the  first  component  of  the  state  from  the  robust  Kalman  filter 
applied  to  measurements  from  this  uncertainty  system.  Lower  and  upper  bounds  for  the 
possible  values  of  the  state  have  also  been  plotted.  The  bounds  on  the  state  estimate  were 
obtained  by  numerically  finding  the  largest  value  of  the  first  component  of  the  state  in  the 
solution  set  (5.7). 


to  20  30  40  50  GO  70  80  90  100 

Time,  k 


Figure  3  (U):  Results  of  Robust  Kalman  Filter. 

It  is  interesting  to  compare  the  performance  of  the  robust  Kalman  filter  and  the  standard 
Kalman  filter,  based  on  the  nominal  system  (ie.  with  A*.  =  0),  for  this  uncertainty  system 
(see  Figure  4).  The  performance  of  the  standard  Kalman  filter  is  very  similar  to  the  robust 
Kalman  filter  and  this  suggests  that  there  is  no  need  for  robust  filtering  with  this  system. 
Increasing  the  energy  of  the  noise  sequences  does  not  result  in  a  situation  where  the  robust 
filter  is  significantly  better  than  the  standard  Kalman  filter. 

6  Particle  Filtering 

An  alternative  approach  to  the  filtering  problem  for  general  non-linear  systems  is  the 
particle  filter.  The  previous  approaches  in  this  report  could  be  called  parametric  (or 
model  based)  techniques.  Unlike  the  previous  approaches,  the  particle  filter  attempts 
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Figure  4  (U):  Comparison  of  Robust  Kalman  Filter  and  the  Kalman  Filter. 

to  estimate  the  whole  posterior  PDF  rather  than  particular  statistics  of  the  posterior 
PDF.  The  particle  filter  is  a  non-parametric  technique  because  it  does  not  attempt  to 
parameterise  the  PDF. 

The  term  particle  filter  includes  the  condensation  algorithm.  Bayesian  bootstrap  [16]  or 
sampling  importance  resampling  filter  [15].  All  these  approaches  represent  the  posterior 
probability  density  of  the  system  state  by  a  system  of  particles  which  evolve  according  to 
the  non-linear  system. 

In  general,  a  very  large  number  of  particles  may  be  required  to  adequately  represent  the 
evolution  of  the  system.  However,  it  can  be  shown  that  any  non-linear  system  can  be 
approximated  by  a  particle  filter  if  the  number  of  particles  is  large  enough. 

6.1  Non-linear  System  Model 

We  introduce  a  slightly  more  general  non-linear  model.  Again  x k  and  yk  are  the  state 
process  and  observation  process  respectively.  Then  we  define  the  system  for  k  6  Z+  as 
follows: 


Xk+I  =  fk{xk-,Wk)  and 

yk  =  hk(  xk.vk)  (6.1) 

where  wk  and  vk  are  noise  terms  with  known  distributions  (not  necessarily  Gaussian). 
Given  the  initial  PDF  of  the  system,  i.e.  p{x$\y-\)  =  p(x o),  these  density  functions  can 
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in  general  be  calculated  for  k  >  0  as  follows 

p{xk\yk-i)  -  J  p{xk\xk-i)p{xk-\\yk-i)  and 

p{yk\xk)p{xk\yk-i) 


p(xk\yk) 


where 


p(Vk\yk-i) 

p(yk\yk~i)  =  j p{yk\xk)p{xk\yk-i)- ' 


(6.2) 

(6.3) 


(6.4) 


In  the  special  case  of  linear  dynamics  and  p{xo)  having  a  Gaussian  distribution  then 
evaluation  of  the  integrals  can  be  simplified  and  this  leads  to  the  Kalman  Filter.  In  general 
these  integrals  can  not  be  analytically  evaluated  and  the  PDF  must  be  approximated 
numerically.  The  particle  filter  technique  is  one  way  of  developing  a  numeric  approximation 
to  the  PDF. 


6.2  Importance  Sampling 


The  particle  filter  approach  provides  a  Monte  Carlo  approximation  to  the  PDF  that  con¬ 
sists  of  a  set  of  random  nodes  in  the  state  space  sk  for  i  =  1. . . . ,  Ns  (termed  the  support) 
and  a  set  of  associated  weights.  w\  for  t  =  1, . . . ,  Ns ,  which  sum  to  1  (see  Figure  5). 


The  objective  of  choosing  the  weights  and  support  is  to  provide  an  approximation  for  the 
PDF  such  that 


Ns  . 

Y.9(sk)wk~  g{xk)p{xk)dxk 

i—\  ^ 


(6.5) 


for  typical  functions  g  of  the  state  space.  This  approximation  is  in  the  sense  that  the 
left-hand  side  equals  the  right-hand  side  as  Ns 


oo. 


In  importance  sampling  the  support,  s\,  is  obtained  by  sampling  values  independently 
from  a  probability  measure  p{xk)  (termed  the  importance  PDF)  and  attaching  weights 

p(4)/p(4)  (66) 


wk 


Standard  Importance  Sampling  Algorithm 


The  following  algorithm  is  repeated  from  [15].  This  algorithm  uses  the  prior  importance 
function  as  mentioned  in  [16].  This  algorithm  assumes  that  p(xk+i\xk),  p[yk\xk )  and p(x o) 
are  known. 
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Figure  5  (U):  Graphical  interpretation  of  a  particle  filter 


1.  (Initialisation)  Sample  from  p{xo)  to  obtain  initial  support  Assign  weights  w\  — 
l/Ns . 

2.  (Preliminaries  on  step  k)  Generate  an  approximation  of  any  required  statistics  from 
the  support  and  weight  (4-i-^i-i)  approximation  for  p(xfc_1|>’fc_1)  using  (6.5). 

3.  (Update  using  prior  importance  function)  Now  generate  the  next  set  of  support 
points  from  the  model  dynamics,  that  is 

4  =  /k-i(4-i>*4-i)  (6-7) 


where  wlk_l  is  sampled  from  the  noise  distribution  and  weights 


pjykK 

Ef=ip(yk\s3k 


(6.8) 


(6.9) 
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Hence,  the  mean  and  variance  of  %k  can  be  estimated  as 

Ns 

E«i(4) 

z~l 
Ns 

^2wl(sk -^fe|fc)(4 -Xk\k)'-  (6-10) 

2—1 

A  major  limitation  of  the  standard  importance  sampling  is  the  degeneracy  of  the  algorithm 
[16].  As  k  increases  the  support  s j.  tends  to  be  concentrated  on  a  same  section  of  the  state 
space  (this  is  equivalent  to  having  a  smaller  Ns).  This  effect  is  shown  in  Figure  5  where 
from  time  k  to  time  k  +  1  the  support  has  been  concentrated  towards  the  centre.  It  has 
been  shown  that  the  best  approximation  occurs  when  the  variance  of  the  weights  is  as 
small  as  possible  [16].  The  performance  of  the  importance  sampling  algorithm  can  be 
improved  by  introducing  the  following  resampling  step 

3.5  (Resample)  The  probability  density  function  represented  by  ( s\,wlk )  is  resampled  to 
an  equally  weighted  support  set  ( s\ .  (Ns)~l). 

Remarks 

6.  The  above  algorithm  uses  the  prior  importance  function  which  is  very  sensitive  to 
outliers  and  in  many  situations  an  alternative  importance  function  shoidd  be  con¬ 
sidered,  see  [16]. 

7.  Further  extensions  including  stratified  sampling  [15],  the  optimal  importance  func¬ 
tion  [16],  linearised  importance  functions  [16]  and  brandling  particle  systems  [17] 
may  offer  significant  improvement  over  the  importance  sampling  algorithm  presented 
above. 

8.  Convergence  proofs  as  Ns  — >  oo  are  given  in  [16]  and  [17]. 

9.  Particle  filter  methods  may  require  very  large  numbers  of  particles  to  ensure  a  rea¬ 
sonable  approximation  is  obtained,  see  [17]  for  a  low  dimension  example. 

10.  Particle  filters  can  often  be  implemented  in  a  parallel  manner. 

11.  Particle  filters  can  be  sensitive  to  outliers  [18]  and  the  particle  filter  requires  knowl¬ 
edge  of  the  initial  PDF  much  like  the  extended  Kalman  filter  [15,  16]. 


Xk\k  ~ 

h\k  ~ 
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6.3  Hidden  Markov  Model  Approximation  of  the  PDF 

Another  related  method  for  approximating  the  evolution  of  the  probability  density  function 
is  to  approximate  the  PDF  as  a  grid  of  points  which  evolve  by  the  state  equations  [20], 
see  Figure  6.  That  is,  the  PDF  is  approximated  by  a  probability  mass  function  where  the 
probability  of  being  in  particular  regions  of  the  state  space  are  represented  by  a  single 
number.  Further,  all  points  in  the  region  are  approximated  by  a  single  point  in  the  region. 
Under  this  representation,  the  probability  of  being  in  a  region  of  the  state  space  is  known, 
but  it  is  not  possible  to  determine  the  probability  of  being  at  a  particular  point  in  the 
region. 

Under  a  few  assumptions,  the  grid  of  points  (or  discretisation  of  the  state  space)  can 
be  considered  a  hidden  Markov  model  to  which  the  corresponding  filtering  theory  can 
be  applied,  see  [21]  for  filtering  theory  for  HMMs.  Although  similar  to  the  particle  filter 
approach  this  method  differs  in  that  the  grid  of  points  represents  regions  of  the  state  space 
(rather  than  fixed  points)  and  the  grid  is  fixed  over  time. 

To  develop  a  HMM  approximation  of  the  non-linear  state  equations  we  introduce  the 
following  notation.  Let  Xk  denote  the  state  of  a  discrete  Markov  state  (or  Markov  chain) 
at  time  k.  Let  {i?i, . . . ,  R^}  denote  N  regions  of  the  state  space  represented  by  the  N 
states  of  the  discrete  Markov  state.  That  is,  if  xk  €  Ri  then  Xk  =  i.  The  evolution  of  a 
discrete  Markov  state  is  described  by  a  state  transition  matrix  Ak  defined  as  follows: 

4':=  P(Xk+1=i\Xk=j) 

where  is  the  ijth  element  of  Ak  £  R^xN. 

To  approximate  the  evolution  of  xk  by  a  discrete  state  we  approximate  Ak  as  follows 

ij  _  Jjii  Irj  Pixk\xk-i)dxk^idxk 

k  f  JR.p{xk\xk-i)dxk-idxk 

It  is  assumed  that  p{xk\xk-\)  is  known  and  hence  Ak  defined  this  way  can  always  be 
calculated.  Once  an  initial  value  Xq  is  given,  the  discrete  Markov  state  approximation  of 
the  continuous  state  is  completely  specified. 

Now  an  approximation  of  the  non-linear  observation  process  is  required.  The  observation 
process  hk(.. .)  can  be  approximated  be  defining  a  matrix  Ck  as  follows: 

Ci  =  hk(x\  0) 
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where  Ck  is  the  ith  element  of  Ck  and  xl  is  a  representative  value  of  xk  in  the  region  R4 
(for  example  the  middle  of  the  region).  Then  the  observation  process  can  be  approximated 


Vk  ~  Ckk  +  vk. 

Standard  HMM  filtering  solutions  [21]  on  the  HMM  approximation  can  be  applied  if  vk 
is  approximated  as  a  Gaussian  random  variable.  Let  X\  denote  P{Xk  —  i\{yo,  -  ■  ■  ,yk})- 


Then  the  HMM  filter  is 


where 


j=i 


Nk  =  'Ep(Vk\Xk  =  i)'EAi*Xik_l. 
i= 1  j= 1 


(6.11) 


(6.12) 


This  filter  can  be  used  to  estimate  the  probability  of  the  state  being  in  each  region  (or 
the  PDF  of  the  discretised  system).  Estimates  of  the  state  value  or  other  statistics  of  the 
state  can  be  formed  from  Xk.  For  any  general  statistic,  E[g(xk)\{yo,  -  -  -  Vk}]-,  the  following 


estimate  can  be  used: 


E[g(xk)\{y0.,...,yk}]^Y,9(^l 


In  particular,  the  conditional  mean  estimate  of  the  state  is 


(6.13) 


(6.14) 


The  HMM  model  that  is  developed  is  only  an  approximation  of  the  true  system  but  as 
the  number  of  grid  points  increase  the  quality  of  the  approximation  should  improve. 

The  advantage  of  applying  HMM  filters  to  the  discretised  state  space  system  is  that  HMM 
filters  are  optimal  in  a  conditional  mean  sense  (on  the  discretised  system)  for  any  non¬ 
linear  state  equations  [21].  If  the  discretised  system  represents  the  true  system  well,  then 
the  HMM  filter  should  give  good  results.  An  additional  advantage  of  the  HMM  filtering 
approach  is  that  it  does  not  suffer  degeneracy  in  the  same  way  as  the  particle  filter.  The 
grid  is  fixed  in  state  space  and  hence  the  approximation  to  the  PDF  does  not  adapt  as 
the  nature  of  the  PDF  changes  and  hence  degeneracy  does  not  occur.  Unfortunately,  the 
fixed  nature  of  the  grid  limits  the  accuracy  of  the  approximation  and  the  particle  filter 
approach  may  perform  better  if  the  degeneracy  can  be  controlled. 
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The  major  disadvantage  of  the  HMM  approach  is  that  the  state  space  can  only  be  discre- 
tised  to  a  fixed  grid  of  points  when  the  variation  in  state  variable  can  be  bounded  (because 
a  grid  of  points  can  only  represent  a  finite  region).  This  limits  application  of  the  HMM 
approach  to  situations  where  the  state  space  is  naturally  bounded. 
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Figure  6  (U):  Graphical  interpretation  of  a  HMM  filter.  The  height  of  the  bars  represents 
the  probability  of  being  in  the  region  of  state  space  represented  by  the  width  of  the  bars. 

Although  there  are  no  solid  theoretical  results  to  support  the  approximation  of  non-linear 
systems  by  HMMs  there  are  several  applications  where  this  technique  has  been  successfully 
applied.  These  applications  include  bearing  only  tracking  problems  [22],  frequency  tracking 
problems  [23],  and  phase  tracking  problems. 


7  Application:  Target  Tracking 

The  application  presented  in  this  section  is  motivated  by  work  being  done  in  the  Guid¬ 
ance  and  Control  group  on  the  optimal  precision  guidance  control  problem.  This  control 
problem  describes  the  terminal  phase  of  a  interceptor-target  engagement.  In  this  section 
we  consider  the  filtering  or  state  estimation  problem  related  to  the  control  problem. 

For  simplicity  consider  an  engagement  defined  in  continuous  time  and  let  the  following 
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definitions  be  in  a  2-D  Euclidean  frame.  Let  (a :fy{)  and  (xf,  yf)  be  the  position  of  the 
interceptor  and  target  respectively.  Then  let  (u(.v{),  ( uf.vf ),  (a[,  b{)  and  ( af ,  bf )  be  the 
velocity  and  acceleration  of  the  interceptor  and  target  respectively. 


Observations  of  the  engagement  are  commonly  related  to  the  relative  dynamics  of  the  inter¬ 
ceptor  and  target  so  we  introduce  the  following  state  variable,  Xt  :=  [xt,  yt,  utl  vt,  af ,  bf], 
where  xt  xf  —  x{  etc.  The  dynamics  of  the  state  can  be  expressed  as  follows 


dX t 
dt 


dXt 

dt 


■  0 

0 

1 

0 

0 

0  ■ 

'  0 
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0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 
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U), 


T,long 

t 

T,lat 


(7.1) 


where  Of  —  tan  1  ( vf  / uf )  is  the  target  heading  angle,  ut  :=  [a(,  bf]',  and  txt  :=  [ujf,lon9.uif,lat]. 
Although  target  acceleration  is  deterministically  controlled  by  the  target,  in  this  model  the 


target  acceleration  has  been  approximated  by  a  “jinking”  type  model  through  the  noises 
uT,iong  and  uT,lat.  This  acceleration  model  is  simplistic  (see  [19]  for  more  realistic  target 


models)  but  is  a  reasonable  representation  in  some  situations. 


Assume  that  the  state  is  observed  at  evenly  spaced  distinct  time  instants  to,  h,  ■  •  • ,  4-  — 
Let  index  k  denote  the  fcth  observation  corresponding  to  the  time  instant  t  =  t*.  Consider 
the  following  observation  process 


Zk  =  f{^tk-,wf,w°h) 


Rk  +  Rkwf 
Ok  +  w°k 


(7.2) 


where  Rk  —  \Jxf  +  yf ,  Of  =  tan  l(ytk/xtk)  and  wf,w6k  are  uncorrelated  zero-mean 
Gaussian  noises  with  variances  o\  and  Og  respectively. 


It  is  useful  to  consider  a  discrete-time  representation  of  the  continuous-time  state  equa¬ 
tion  (7.1)  obtained  through  sampling  theory.  Let  h  =  tk  —  tk~  \  then  using  sample  hold 
approximation  the  discrete-time  state  equation  is 

xtk+ 1  =  eAhXtk  +  GtkG)k  or 

^jfc+i  =  AXk  +  Gkuk  (7.3) 


where  uk  =  l/h  fff+l  utdt  and  Xk  etc.  denote  the  discrete- time  representation  of  Xk  etc.. 
The  variance  of  uk  is  l/h  times  the  variance  of  o>t- 
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We  consider  the  engagement  shown  in  Figure  7.  The  engagement  commences  at  a  dis¬ 
tance  of  5000  m.  The  interceptor  is  traveling  at  a  velocity  of  1000  m/s  in  a  direction 
36°  (measured  clockwise)  from  a  line  drawn  between  the  interceptor  and  target.  The  in¬ 
terceptor  is  traveling  at  a  velocity  of  660  m/s  in  a  direction  of  120°  from  the  same  line. 
Hence,  the  initial  conditions  axe  (x^Pq^u^Vq)  —  (0, 0, 1000 cos(36°),  1000 sin(450))  and 
(xQ,yQ,UQ,VQ)  =  (5000,0, 660  cos  (120°),  660  sin(  120°))  where  distances  are  in  units  of  m 
and  velocities  are  in  units  of  m/s.  Assume  no  control  action  is  taken  by  the  interceptor. 


Figure  7  (U):  Engagement  configuration.  The  interceptor  and  target  are  roughly  heading 
towards  the  same  point  (collision  will  not  occur  unless  a  manoeuvre  is  performed). 


7.1  Extended  Kalman  Filter  Approach 


To  apply  the  extended  Kalman  filter  to  this  problem  we  obtain  a  linear  approximation 
for  the  state  equation.  We  approximate  the  non-linearity  in  the  driving  term  as  a  time- 
varying  linear  functions  (that  is  Ak  =  A  and  Bk  =  G(Xk\k_i)  where  Xk\k-v  is  the  one- 
step-ahead  prediction  of  Xk).  The  measurement  equation  (7.2)  is  non-linear  in  the  state 
and  linearisation  at  Xk\k-\  gives 


Ck 


dck(X) 


dX 


x=x 


k\k-\ 


%k\k—l/ ^k\k—  1 
~Vk\k-l/^k\k-l 
0 
0 
0 
0 


yk\k-i/Rk\k-\ 
%k\k-i!  Rk\k-\ 

o 

o 

o 

o 


(7.4) 
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The  extended  Kalman  filter  can  now  be  implemented  using  the  recursions  (4.3)  stated 
above. 

The  extended  Kalman  filter  was  used  in  a  simulated  engagement  to  estimate  the  target 
state  at  each  time  instant.  The  sampling  rate  of  the  simulation  was  0.001  Hz.  The  initial 
estimate  of  the  target  position  was  (5500, 500)  m  and  velocity  errors  of  5  ms~l  in  both  x 
and  y  directions  were  present.  The  range  measurement  noise  variance  was  0.25i?fc,  where 
Rk  is  the  range  at  time  k.  and  the  angle  measurement  noise  variance  was  0.25.  The  target 
was  assumed  to  be  non-accelerating. 

Figure  8  shows  a  plot  of  both  the  target  and  interceptor  trajectories  as  well  as  the  inter¬ 
ceptor’s  estimate  of  the  target’s  position.  The  initial  position  error  quickly  reduces  and 
after  4.39  s,  when  the  interceptor  and  target  are  71  m  apart  (which  is  the  closest  distance 
achieved)  the  error  in  the  estimated  target  position  is  0.67  m.  The  extended  Kalman  filter 
also  provides  estimates  of  the  target’s  velocity. 


Figure  8  (U):  Estimate  Target  Position 
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7.1.1  Stability  of  the  Extended  Kalman  Filter 


The  stability  of  the  extended  Kalman  filter  in  this  situation  can  be  examined  using  the 
results  in  Section  4.2.  Because  the  state  equations  are  linear  the  conditions  for  stability 
stated  in  Theorem  1  simplify  to 


e  =  min 


pra 

2p2Kx 


(2(i+f  )+^y') 


(7.5) 


where  we  have  used  c  =  1,  a  =  1,  is  unbounded  and  —  0. 


To  determine  whether  stability  of  the  extended  Kalman  filter  can  be  assured  for  a  partic¬ 
ular  initial  error  value  we  tested  values  of  ex  in  (7.5).  We  investigated  stability  against 
initial  errors  in  the  y  position  coordinate  (assuming  no  error  in  xq).  Figure  9  shows  the 
values  of  e  achieved  for  various  values  of  ex  (note  that  this  figure  shows  only  the  stability 
at  the  initial  time  instant  and  the  stability  of  the  filter  at  later  time  instants  needs  to 
be  tested  separately).  From  Figure  9,  stability  of  the  EKF  can  be  guaranteed  for  initial 
errors  in  y  less  that  180  m.  Using  (7.5)  it  can  be  shown  that  when  y0  is  known,  errors  in 
xq  do  not  cause  the  EKF  to  diverge. 


Figure  9  (U):  The  initial  errors  in  yo  for  which  the  EKF  is  guaranteed  to  converge. 


We  know  from  the  simulation  results  that  the  EKF  converged  from  initial  errors  of  500m 
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in  both  axis.  This  demonstrates  that  although  useful,  the  bounds  produced  by  (7.5)  axe 
conservative. 

7.2  Particle  Filter  and  HMM  Approaches 

The  particle  filter  was  applied  to  the  same  target  trajectory  estimation  problem.  The  noise 
variance  was  lowered  to  0.05 Rk  and  0.05  in  the  range  and  angle  measurements  respectively. 
A  system  of  1000  particles  was  used  to  represent  the  evolution  of  the  PDF.  The  initial 
support  of  the  particle  filter  was  sampled  from  a  Gaussian  density  function  with  a  mean 
displaced  by  the  amount  (500,  500, 0.5, 0.5, 0, 0)  from  the  true  initial  state  with  a  covariance 
matrix  diag([5002, 5002, 0.52, 0.55, 1CT6, 10~6]).  Here  diag(X)  is  the  operation  of  creating 
a  diagonal  matrix  from  the  vector  X  (if  A  is  a  vector)  or  the  operation  of  making  a  vector 
from  the  diagonal  of  the  matrix  X  (if  A  is  a  matrix). 

A  new  support  set  was  obtained  at  every  10  time  instants  by  resampling  from  a  Gaussian 
density  function  with  the  following  mean  and  covariance: 

Ns 

Xk  =  J2skwk 
2=1 
Ns 

var(xk )  =  5Z(4  ~Xk)(slk  -  xk)'wk,  (7.6) 

2=1 

where  xk  is  an  estimate  of  the  state  and  var{xk)  is  the  co- variance  matrix  for  xk. 

The  observation  noise  was  inflated  to  included  the  effect  state  estimation  errors  in  the 
following  way: 

var(Rk)  =  0.05  Rk  +  C\var{xk)C[ 

var(6k )  —  0.05  +  C2var{xk)C'2  (7.7) 

where  C\  and  Oi  are  the  first  and  second  rows  of  the  linearisation  matrix  C  used  in  the 
EKF. 

The  particle  filter  was  able  to  estimate  the  trajectory  when  initialised  on  the  true  initial 
conditions;  however,  if  initialised  with  any  error,  this  error  remained  for  the  length  of  the 
simulation  and  the  filter  was  unable  to  correct  for  tliis  error.  These  simulation  results  sug¬ 
gests  that  when  the  initial  PDF  is  not  known  the  filter  is  neither  divergent  nor  convergent 
but  not  very  useful  for  this  application. 
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HMM  filter 

To  apply  the  HMM  approach  to  this  filtering  problem  requires  discretisation  of  the  state 
space.  The  HMM  approach  is  not  computationally  tractable  for  the  above  target  trajectory 
estimation  problem.  To  illustrate  the  computational  effort  required  consider  the  effort 
required  for  even  the  most  basic  and  coarse  approximation. 

Assume  that  the  velocity  and  acceleration  are  known  (which  is  a  fairly  restrictive  assump¬ 
tion  in  target  tracking  problems).  For  the  configuration  above,  it  is  reasonable  to  bound 
the  position  space  to  (-4500, 5500)  in  the  x-axis  and  (0, 1000)  in  the  y-axis.  If  this  space 
is  coarsely  discretised  into  10  m  by  10  m  regions  then  1  x  104  discrete  states  are  required. 
The  HMM  filter  on  a  1  x  104  state  process  will  require  in  the  order  of  1  x  108  calculations 
per  time  instant.  Successful  filtering  is  likely  to  require  even  finer  discretisation  than  10  m 
and  hence  computational  requirements  axe  likely  to  be  significant  less  tractable. 

8  Conclusions 

This  report  presented  a  review  of  recent  non-linear  and  robust  filtering  results  for  stochas¬ 
tic  systems.  Stability  results  for  the  extended  Kalman  filter  and  a  robust  Kalman  filtering 
solution  were  presented.  The  report  also  examined  a  recent  non-parametric  filtering  tech¬ 
nique  known  as  the  particle  filter.  Finally,  some  simulation  examples  were  presented  that 
demonstrate  the  performance  of  several  filters. 

In  many  applications  the  extended  Kalman  filter  may  offer  the  best  filtering  solution,  but 
in  highly  non-linear  problems  this  filter  is  unlikely  to  perform  well.  For  more  non-linear 
problems  there  are  various  higher-order-model  approaches  that  offer  suboptimal  filtering 
solutions.  For  the  most  complex  problems,  generic  approaches  such  as  the  non-parametric 
particle  filter  may  be  appropriate,  admittedly  at  a  heavy  computational  cost. 

All  of  these  approaches  assume  certain  knowledge  of  the  state  and  measurement  processes, 
which  is  unrealistic  in  practice.  On  the  other  hand,  robust  Kalman  filtering  and  similar 
techniques  mitigate  for  uncertainty  in  the  system  model,  but  this  mitigation  is  generally 
at  some  performance  loss. 

Out  of  all  these  possible  filtering  approaches,  no  one  approach  is  superior  to  the  all  others 
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and  finding  the  most  appropriate  filter  many  require  substantial  investigation. 
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